/*! \copyright 2024 Zorxx Software. All rights reserved.
 *  \license This file is released under the MIT License. See the LICENSE file for details.
 *  \brief rtci2c library esp-idf example application
 */

#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
#include "esp_freertos_hooks.h"
#include "freertos/semphr.h"
#include "esp_system.h"

#include <errno.h>
#include "esp_err.h"
#include "esp_log.h"

#include "driver/gpio.h"
#include "soc/gpio_num.h"

#include "myiic.h"

#include "qmc63xx.h"

#define TAG "qmc63xx"


    
#define QMC63XX_SCL_IO          				(gpio_num_t)39     /*!< gpio number for I2C master clock */
#define QMC63XX_SDA_IO          				(gpio_num_t)40     /*!< gpio number for I2C master data  */
#define QMC63XX_FREQ_HZ    		   				 100000            /*!< I2C master clock frequency 100kHz and 400kHz*/







//Register Map
//QMC6308 QMC6309 QMC6310(包过QMC6310U和QMC6310N)
#define QMC63XX_ADDRESS                   		(0x7C)//QMC6308:0x2C,QMC6309:0x7C,QMC6310U:0x1C,QMC6310N:0x3C
#define QMC63XX_DEFAULT_ID                      (0x90)//芯片id//QMC6309:0x90, QMC6310:0x80

#define QMC63XX_REG_CHIP_ID                     (0x00)//芯片id(R 只读)
#define QMC63XX_REG_LSB_DX                      (0X01)//x轴低位(R 只读)
#define QMC63XX_REG_MSB_DX                      (0X02)//x轴高位(R 只读)
#define QMC63XX_REG_LSB_DY                      (0X03)//y轴低位(R 只读)
#define QMC63XX_REG_MSB_DY                      (0X04)//y轴高位(R 只读)
#define QMC63XX_REG_LSB_DZ                      (0X05)//z轴低位(R 只读)
#define QMC63XX_REG_MSB_DZ                      (0X06)//z轴高位(R 只读)
//以上寄存器含义 在QMC6308 QMC6309 QMC6310U QMC6310N系列芯片完全兼容 
#define QMC63XX_REG_STATUS                      (0X09)// 状态寄存器(R 只读)   //OVFL<1> DRDY<0>位含义完全兼容
#define QMC63XX_REG_CMD1                        (0x0A)//控制寄存器1(R/W 读/写)//MODE<1:0>位含义完全兼容 其它位OSR2 OSR1 ODR各不相同
#define QMC63XX_REG_CMD2                        (0x0B)//控制寄存器2(R/W 读/写)//SOFT_RST<7> SET/RESET MODE<1:0>含义完全兼容 其它位各不相同
//以上寄存器含义 在QMC6308 QMC6309 QMC6310U QMC6310N系列芯片上部分兼容  
#define QMC6309_REG_SELF_DX                     (0x13)//x轴低位(R 只读)
#define QMC6309_REG_SELF_DY                     (0x14)//y轴低位(R 只读)
#define QMC6309_REG_SELF_DZ                     (0x15)//z轴低位(R 只读)
//以上寄存器(13H~15H)是QMC6309才有的寄存器，用于自检;13H~15H store the X,Y and Z self-test data under self-test mode separately.
//QMC6310的自检 依然用00~06读取

//寄存器位详情
//-------------------------------------------------//
//状态寄存器 Status Register(0X09)
#define QMC63XX_REG09_DRDY  			0x01//         含义DRDY: “0”: no new data, “1”: new data is ready   新数据是否准备就绪,就绪则可以读取了
#define QMC63XX_REG09_OVFL  			0x02//         含义OVFL: “0”: no data overflow occurs, “1”: data overflow occurs 数据是否发生溢出
#define QMC6309_REG09_ST_RDY  			0x04//       含义ST_RDY: “0”: selftest not done yet; “1”: selftest is done, selftest data is ready for reading 表示内置自检测量的状态                                       
#define QMC6309_REG09_NVM_RDY  			0x10//      含义NVM_RDY: “0”: NVM not ready for access; “1”: NVM ready for access 表示内置非易失性存储器的状态
#define QMC6309_REG09_NVM_LOAD_DONE  	0x20//含义NVM_LOAD_DONE: “0”: data loading from NVM not finished; “1”: data loading from NVM finished 表示从内置非易失性存储器加载数据的状态
//-------------------------------------------------//
//控制寄存器1 Control Registers1(0x0A)
////MODE<1:0>
//#define QMC63XX_REG0A_Suspend_Mode      0x00//Suspend Mode 		待机模式
//#define QMC63XX_REG0A_Normal_Mode  		0x01//Normal Mode  		正常模式
//#define QMC63XX_REG0A_Single_Mode  		0x02//Single Mode  		单次模式
//#define QMC63XX_REG0A_Continuous_Mode  	0x03//Continuous Mode   连续模式
//OSR:Over sample Rate 过采样率
//ODR:Output data rate 输出数据速率
//QMC6309
//----OSR1<1>   0:256  1:128
//----OSR2<7:5> Continuous Mode: 000:1 001:2 010:4 011:8;Others: Reserved for Future Use
//QMC6309
//----OSR1<4:3> 00:8 01:4 10:2 11:1
//----OSR2<7:5> 000:1 001:2 010:4 011:8 100:16 101:16 110:16 111:16
//QMC6310
//---- ODR<3:2> 00:10Hz 01:50Hz 10:100Hz 11:200Hz
//----OSR1<5:4> 00:8 01:4 10:2 11:1
//----OSR2<7:6> 00:1 01:2 10:4 11:8
//-------------------------------------------------//
//控制寄存器2 Control Registers2(0x0B)
#define QMC63XX_REG0B_SOFT_RST      			0x80 //复位的是寄存器值//0: no reset;1 Soft reset, restore default value of all registers
#define QMC63XX_REG0B_Mode_Set_and_reset_on     0x00
#define QMC63XX_REG0B_Mode_Set_only_on     		0x01
//#define QMC63XX_REG0B_Mode_Set_and_reset_off    0x02 QMC6309无意义 其它型号的和0x03的功能相同
#define QMC63XX_REG0B_Mode_Set_and_reset_off    0x03

/*
	寄存器01H~06H
	Registers 01H~06H store the measurement data from each axis magnetic sensor in each working mode. In the 
	normal mode, the output data is refreshed periodically based on the data update rate ODR setup in control 
	registers 1. The data stays the same, regardless of reading status through I2C, until new data replaces them. 
	Each axis has 16-bit data width in 2’s complement, i.e., MSB of 02H/04H/06H indicates the sign of each axis. The 
	output data of each channel saturates at -32768 and 32767
	寄存器01H~06H存储每种工作模式下每个轴磁传感器的测量数据。
	在正常模式下，基于控制寄存器1中设置的数据更新速率ODR定期刷新输出数据。
	无论通过I2C读取状态如何，数据都保持不变，直到新数据替换它们。
	每个轴在2的补码中具有16位数据宽度，即02H/04H/06H的MSB表示每个轴的符号。
	每个通道的输出数据在-32768和32767处饱和
	
	Register 13H~15H store the X,Y and Z self-test data under self-test mode separately. The selftest data of each channel
	寄存器是QMC6309才有的寄存器，寄存器13H~15H分别存储自检模式下的X、Y和Z自检数据。每个的自检数据通道
*/
//
//static i2c_bus_device_handle_t qmc63xx_handle=NULL;

esp_err_t qmc63xx_init(void)
{
	my_iic_init();
		
	unsigned char chipid=qmc63xx_getChipID();
	if(chipid==0x90)
	{
	    ESP_LOGI(TAG,"chip type=qmc6309;chip id =0x%02x\r\n",chipid);		
	}
	else if(chipid==0x80)
	{
	    ESP_LOGI(TAG,"chip type=qmc6310;chip id =0x%02x\r\n",chipid);		
	}
	else 
	{
	    ESP_LOGI(TAG,"chip type=qmc6308;chip id =0x%02x\r\n",chipid);	
	}	
	
	return ESP_OK;		
}
// get the sensor chip id 
uint8_t qmc63xx_getChipID(void)
{
	uint8_t chipid;
    ESP_ERROR_CHECK(my_iic_read_byte(QMC63XX_ADDRESS,QMC63XX_REG_CHIP_ID, &chipid));
    return chipid;
}
// get the sensor status
uint8_t qmc63xx_getStatus(void)
{
	uint8_t status;
    ESP_ERROR_CHECK(my_iic_read_byte(QMC63XX_ADDRESS,QMC63XX_REG_STATUS, &status));
    return status;
}
// get new data is ready?
bool qmc63xx_isDataReady(void)
{
	uint8_t status;
    ESP_ERROR_CHECK(my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_STATUS, (uint8_t *)&status));
    if(status & QMC63XX_REG09_DRDY) 
    {
        return true;
    }

    return false;
}
// get data overflow occurs?
bool qmc63xx_isDataOverflow(void)
{
	uint8_t status;
    ESP_ERROR_CHECK(my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_STATUS, (uint8_t *)&status));
    if(status & QMC63XX_REG09_OVFL) 
    {
        return true;
    }

    return false;
}
// Set the sensor mode
esp_err_t qmc63xx_setMode(SensorMode mode)
{
	esp_err_t err;
	unsigned char data[2] = {0};

	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
	data[1] = (data[0]& 0xFC) | mode;//因为用的是MODE<1:0> 所以用0x03屏蔽 新值放到了data[1]里面
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0A = [%02x]->[%02x]", data[0],data[1]);

	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, data[1]);
	ESP_ERROR_CHECK(err);	
	
	return ESP_OK;
}
//Set the sensor Set/Reset Mode 
esp_err_t qmc63xx_setResetMode(SetResetMode mode)
{
	esp_err_t err;
	unsigned char data[2] = {0};

	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
	data[1] = (data[0]& 0xFC) | mode;//因为用的是Set/Reset MODE<1:0> 所以用0x03屏蔽 新值放到了data[1]里面
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0B = [%02x]->[%02x]", data[0],data[1]);

	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, data[1]);
	ESP_ERROR_CHECK(err);	
	
	return ESP_OK;
}

#if (QMC63XX_CHIP_QMC6309|QMC63XX_CHIP_QMC6310)
//Set the sensor ODR(Output data rate) 
esp_err_t qmc63xx_setDataOutputRate(OutputRate odr)
{
	esp_err_t err;
	unsigned char data[2] = {0};
#if QMC63XX_CHIP_QMC6309
//控制寄存器2 Control Registers2(0x0B)	
//ODR[2:0]
	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
	data[1] = (data[0]& 0x8F) | (odr<4);
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0B = [%02x]->[%02x]", data[0],data[1]);

	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, data[1]);
	ESP_ERROR_CHECK(err);	
#elif QMC63XX_CHIP_QMC6310
//控制寄存器2 Control Registers1(0x0A)
//ODR<1:0
	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
	data[1] = (data[0]& 0xF3) | (odr<2);
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0A = [%02x]->[%02x]", data[0],data[1]);

	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, data[1]);
	ESP_ERROR_CHECK(err);	
#endif
	return ESP_OK;
}

//Set the sensor OSR1(Over sample Rate)
esp_err_t qmc63xx_setOverSampleRate(OverSampleRatio osr)
{
	esp_err_t err;
	unsigned char data[2] = {0};

//控制寄存器1 Control Registers1(0x0A)
//OSR1<1:0>
	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
#if QMC63XX_CHIP_QMC6309
	data[1] = (data[0]& 0xE7) | (osr<3);
#elif QMC63XX_CHIP_QMC6310
	data[1] = (data[0]& 0xCF) | (osr<4);
#endif
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0A = [%02x]->[%02x]", data[0],data[1]);
	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, data[1]);
	ESP_ERROR_CHECK(err);	

	return ESP_OK;
}

//Set the sensor DSR(Down sample Rate OSR2)
esp_err_t qmc63xx_setDownSampleRate(DownSampleRatio dsr)
{
	esp_err_t err;
	unsigned char data[2] = {0};

//控制寄存器1 Control Registers1(0x0A)
//QMC6309 OSR2<7:5> 
//QMC6310 OSR2<7:6> 
	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值
#if QMC63XX_CHIP_QMC6309
	data[1] = (data[0]& 0x1F) | (dsr<5);
#elif QMC63XX_CHIP_QMC6310
	data[1] = (data[0]& 0x3F) | (dsr<6);
#endif
	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0A = [%02x]->[%02x]", data[0],data[1]);
	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, data[1]);
	ESP_ERROR_CHECK(err);	

	return ESP_OK;
}

//Set the sensor Full Scale Range(Guass)
esp_err_t qmc63xx_setMagRange(MagRange range)
{
	esp_err_t err;
	unsigned char data[2] = {0};		
//    switch (range) 
//    {
//	    case RANGE_30G:
//	        sensitivity = 0.1;
//	    break;
//	    case RANGE_12G:
//	        sensitivity = 0.04;
//	    break;
//	    case RANGE_8G:
//	        sensitivity = 0.026;
//	     break;
//	    case RANGE_2G:
//	        sensitivity = 0.0066;
//	    break;
//	    default:
//	    break;
//    }

//控制寄存器1 Control Registers1(0x0B)
//RNG[1:0] 位地址一样 但含义不同
	//读取寄存器里面的值
	err=my_iic_read_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, (uint8_t *)&data[0]);
	ESP_ERROR_CHECK(err);
	//修改只想改变的值

	data[1] = (data[0]& 0xF3) | (range<2);

	ESP_LOGI(TAG,"qmc63xx_set_mode, 0x0B = [%02x]->[%02x]", data[0],data[1]);
	//重新写入寄存器
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, data[1]);
	ESP_ERROR_CHECK(err);	

	return ESP_OK;
}
void qmc63xx_configMagnetometer(SensorMode mode, MagRange range, OutputRate odr,OverSampleRatio osr, DownSampleRatio dsr)
{
	qmc63xx_setMode(mode);
	vTaskDelay(50 / portTICK_PERIOD_MS);
		
	//Set the sensor Full Scale Range(Guass)
	qmc63xx_setMagRange(range);	
	vTaskDelay(50 / portTICK_PERIOD_MS);
	
	// Set the sensor ODR(Output data rate) 
	qmc63xx_setDataOutputRate(odr);
	vTaskDelay(50 / portTICK_PERIOD_MS);
	
	//Set the sensor OSR1(Over sample Rate)
	qmc63xx_setOverSampleRate(osr);
	vTaskDelay(50 / portTICK_PERIOD_MS);
	
	//Set the sensor DSR(Down sample Rate OSR2)
	qmc63xx_setDownSampleRate(dsr);
}
#endif

//Continuous Mode
esp_err_t qmc63xx_enable(void)
{
	esp_err_t err=ESP_OK;

	//写入寄存器  
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, 0x00);//(No reset,Output data Rate 1Hz, Field Range 32Guass,Set/Reset On)                                                                        
	//err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD2, 0x10);//(No reset,Output data Rate 10Hz, Field Range 32Guass,Set/Reset On)
	ESP_ERROR_CHECK(err);		
	
	err=my_iic_write_byte(QMC63XX_ADDRESS, QMC63XX_REG_CMD1, 0x63);//(OSR2(DSR)=8,OSR1=8,Continuous Mode)
	ESP_ERROR_CHECK(err);	
			
	return err;
}

esp_err_t qmc63xx_disable(void)
{
	esp_err_t ret=0;
	
	ret = qmc63xx_setMode(MODE_SUSPEND);
	
	return ret;
}
/* X,Y and Z-axis magnetometer data readout
 * param *mag pointer to \ref qmc6308_t structure for x,y,z data readout
 * note data will be read by multi-byte protocol into a 6 byte structure
 */

esp_err_t qmc63xx_read_mag_xyz(QMC63XX_SensorData *sensordata)
{
	esp_err_t res;
	unsigned char mag_data[6];
	int hw_d[3] = {0};
	int t1 = 0;
	int sensitivity = 32000;//Field Range=32Guass

	/* Check status register for data availability */
	while( !qmc63xx_isDataReady()&&(t1<15) )//超时处理
	{
		vTaskDelay(100 / portTICK_PERIOD_MS);
		t1 ++;
	}

	//连续将01H~06H读取出来
	 res=my_iic_read_bytes(QMC63XX_ADDRESS, QMC63XX_REG_LSB_DX, 6, (uint8_t *)&mag_data);
	if(res!=ESP_OK)
	{
		
		return ESP_FAIL;
	}	
	//printf("qmc6308 mag_data[%02x, %02x, %02x, %02x, %02x, %02x]\n",
	//	mag_data[0], mag_data[1], mag_data[2],
	//	mag_data[3], mag_data[4], mag_data[5]);

	hw_d[0] = (short)(((mag_data[1]) << 8) | mag_data[0]);
	hw_d[1] = (short)(((mag_data[3]) << 8) | mag_data[2]);
	hw_d[2] = (short)(((mag_data[5]) << 8) | mag_data[4]);

	//Unit:mG  1G = 100uT = 1000mG
	//so 
	hw_d[0] = hw_d[0] * 1000 /sensitivity;
	hw_d[1] = hw_d[1] * 1000 /sensitivity;
	hw_d[2] = hw_d[2] * 1000 /sensitivity;

	//sprintf("Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);

	sensordata->x = hw_d[0]/10.0f;//unit = uT
	sensordata->y = hw_d[1]/10.0f;
	sensordata->z = hw_d[2]/10.0f;


	return ESP_OK;
}

void qmc63xx_task(void *pvParameter)
{
	QMC63XX_SensorData sensordata;
	qmc63xx_enable();
	
	while(1)
	{
		vTaskDelay(1000 / portTICK_PERIOD_MS);	
			
		qmc63xx_read_mag_xyz(&sensordata);
		ESP_LOGI(TAG,"qmc63xx data=[%6.2f,%6.2f, %6.2f]", sensordata.x, sensordata.y, sensordata.z);
	}	
}
void qmc63xx_example(void)
{
	ESP_ERROR_CHECK(qmc63xx_init());
    xTaskCreate(&qmc63xx_task, "qmc63xx", 4096, NULL,2,NULL);	
}

